% 参数初始化
N = 10; % 天线子阵数目
M = 11;  % 每个子阵中的天线数目
D = 12; % 最大位置范围
Gamma = M; % 最小子阵中天线数，大于M
lambda = 0.01; % 波长    % 假设波长10mm，频率30GHz
d = 0.1; % 天线间距
d_min = (Gamma - 1)*d;    % 最小子阵列中心之间的间距(m) 
S = (M - 1)*d;            % 每个子ULA的整体孔径 1(m)  

r_u = [10, 20].';
r_w = [15, 25].';

dim = 2 * N;
u = rand(dim, 1) * D /2;  % 粒子位置  U的维度是   2 * N     1
Ct_bounds = [0, 0; -6, 6];
% 初始天线位置
u(:) = enforce_constraints(u(:), d_min, Ct_bounds);   % U维度 2N * 1

t = reshape(u, [2, N]);  % 发射天线坐标 [2 × N]
y = t(2,:);

for n = 1:N
  theta_u(n) = atan2(r_u(2) - t(2, n), r_u(1) - t(1, n)); 
  theta_w(n) = atan2(r_w(2) - t(2, n), r_w(1) - t(1, n));
end




% 设置算法参数
max_iter = 100; % 最大迭代次数
tolerance = 1e-6; % 收敛条件
step_size = 0.1; % 步长

% % 计算常数部分 c(y)
% c = @(y, n1, m) - (2*pi/lambda) * (sqrt((r_u(2) - y(n1) ).^2 + r_u(1)^2) - sqrt((r_w(2) - y(n1)).^2 + r_w(1)^2)) + ...
%              (2*pi/lambda) * (m * d * (sin(theta_u(n1)) - sin(theta_w(n1))));
% 
% % 目标函数计算
% cos_func = @(y, n1, m1, n2, m2) cos(c(y, n1, m1) - c(y, n2, m2));  % 非凸目标函数
% sin_func = @(y, n1, m1, n2, m2) sin(c(y, n1, m1) - c(y, n2, m2));  % 非凸目标函数
% 整个迭代过程就是针对y的每一个元素找上界。

% BSUM算法实现

    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s = 0;
y_pre = y;  % 上一次的 t_sub 结果
y_cur = y;   % 当前的 t_sub 结果
epsilon = 0.001;
% 重复迭代直到收敛 迭代参数应该是n1
while true
    s = s + 1;
    n1 = mod(s, N);
    m1 = n1;
    % 求出对于yn1来说位置的最优解，  对于每个不同的n,m都要乘
    % 算y_opt_n1
    % 假设 a 和 b 是预先定义的矩阵或数组
    % n1 和 m1 是已知的索引值
    
    % 初始化分子和分母
    numerator = 0;
    denominator = 0;

    % 遍历所有可能的 (n2, m2) 组合
    for n2 = 1:length(N)
        for m2 = 1:length(M)
            if ~((n2 == n1) && (m2 == m1))
                numerator = numerator + a_n1m1_n2m2(n1, m1, n2, m2) * b_n1m1_n2m2(n1, m1, n2, m2);
                denominator = denominator + a_n1m1_n2m2(n1, m1, n2, m2);
            end
        end
    end

    yopt = numerator / denominator;
    
    % 假设 y_n1_prev_s, Gamma, d, yopt, D, N 是预先定义的变量
    if yopt <= y_pre(n1) + Gamma * d
        y_n1_opt = y_pre(n1) + Gamma * d;
    elseif y_pre(n1) + Gamma * d < yopt && yopt <= (D / 2) - (N - n1) * Gamma * d
        y_n1_opt = yopt;
    else
        y_n1_opt = (D / 2) - (N - n1) * Gamma * d;
    end
    
    y_pre = y_cur;
    y_cur(n1) = y_n1_opt;
    t_pre = [zeros(N,1),y_pre].';
    t_cur = [zeros(N,1),y_cur].';
    g_w_cur = position2williechannel( t_cur, r_w, N, M, lambda, d);
    g_u_cur = position2williechannel( t_cur, r_u, N, M, lambda, d);
    g_w_pre = position2williechannel( t_pre, r_w, N, M, lambda, d);
    g_u_pre = position2williechannel( t_pre, r_u, N, M, lambda, d);
    % 收敛判断
    if abs(abs(g_u_cur * g_w_cur')^2 - abs(g_u_pre * g_w_pre')^2) < epsilon
        break; % 如果满足收敛条件，退出循环
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% 输出最终天线位置
disp('最终天线位置：');
disp(y_cur);

% 可选：绘制天线位置变化图
figure;
plot(1:N, y_cur, '-o');
xlabel('天线编号');
ylabel('位置');
title('优化后的天线位置');

